/*
 * main implementation: use this sample to create your own application
 *
 */

#include "support_common.h" /* include peripheral declarations and more */

#if (CONSOLE_IO_SUPPORT || ENABLE_UART_SUPPORT)
/* Standard IO is only possible if Console or UART support is enabled. */
#include <stdio.h>
#endif
#include <stdlib.h>

#include "hmc5883_support.h"
#include "i2c_support.h"

#define LED_RUNNING		(uint8) (0x01)
#define LED_READING		(uint8) (0x02)

#define STATE_EMPTY		0
#define STATE_OCCUPIED	1

#define Z_THRESHOLD		250

void getXbeeAddress (uint32 *, uint32 *);
void delay_ms (int);

int main(void)
{
	uint32 serialL, serialH;
	
	MagData baseline, data;
	
	uint8 state = STATE_EMPTY;
	uint8 new_state = STATE_EMPTY;

	MCF_GPIO_PORTTC = 0 | LED_RUNNING;
	
	mcf52233_i2c_init();
	hmc5883_write(0x00, 0x70);
	hmc5883_write(0x02, 0x00);
	
	getXbeeAddress(&serialL, &serialH);
	
	hmc5883_read_all(&baseline);
	
	while (1)
	{	
		MCF_GPIO_PORTTC |= LED_READING;
		hmc5883_read_all(&data);
		MCF_GPIO_PORTTC &= ~LED_READING;
		
		if (abs(baseline.getZ() - data.getZ()) > Z_THRESHOLD)
		{
			new_state = STATE_OCCUPIED;
		} 
		else 
		{
			new_state = STATE_EMPTY;
		}
		
		if (state != new_state) 
		{
			state = new_state;
			printf("s\nid=%08X%08X\nstatus=%d\ne\n", serialH, serialL, state);
			
		}

#ifdef DEBUG_ENABLE
		printf("X: %d\n", data.getX());
		printf("Y: %d\n", data.getY());
		printf("Z: %d\n", data.getZ());
		printf("Z_DIFF: %d\n", abs(baseline.getZ() - data.getZ()));
		printf("STATUS: %d\n", state);
		printf("\n\n");
#endif
		
		if ((state == STATE_EMPTY) && 
				(MCF_EPORT0_EPPDR & MCF_EPORT_EPDDR_EPDD4)) 
		{
			MCF_GPIO_PORTTC &= ~LED_RUNNING;
			asm("stop	#0x2000");
			MCF_GPIO_PORTTC |= LED_RUNNING;
			disable_interrupts();
		}
		
		delay_ms(250);
	}
	
	MCF_GPIO_PORTTC = 0x00;
}

void getXbeeAddress(uint32 *addrL, uint32 * addrH) 
{
	char addr_str[2];

	*addrL = 0;
	*addrH = 0;
	
	delay_ms(1000);
	printf("%s", "+++");
	scanf("%2c", addr_str);
	if ((addr_str[0] == 'O') & (addr_str[1] == 'K'))
	{
		printf("ATSL\r");
		scanf("%x", addrL);
		
		printf("ATSH\r");
		scanf("%x", addrH);
		
		printf("ATCN\r");
		scanf("%2c", addr_str);
		if ((addr_str[0] != 'O') | (addr_str[1] != 'K'))
		{
			delay_ms(3000);
		}
	}
}

void delay_ms(int ms)
{
	int i, j;
	for (i = 0; i < ms; i++)
	{
		for (j = 0; j < 3675; j++) {}
	}
}
